# -*- coding: utf-8 -*-
"""
Created on Wed Mar 30 09:48:05 2022

@author: LSY
"""
from Models import PCCPlant
import casadi
import numpy as np
import matplotlib.pyplot as plt
from numpy import random
import timeit
import mpctools as mpc

from Sens_proj_cc import SenMatrix, SensAnal

#np.set_printoptions(precision=4)
#np.set_printoptions(linewidth=400)

# ---------------------------------------------------------------------
# Simulation parameters
# ---------------------------------------------------------------------
rank_xp = True
dt = 60.0 * 10  # Plant simulation time step or sampling time, s # must be same as the sampling time in the data generation
tf = 3600 * 10  # Total simulation time, s # over-ridden
Nt = int(round(tf / dt))  # Number of simulation points # over-ridden
tplot = np.arange(Nt)*10
# ---------------------------------------------------------------------
# Steady state and initial values of the plant
# ---------------------------------------------------------------------
us = np.load('uss.npy')
us = np.append(us,1.0)  # re-write us to include new input
x0 = np.load('x0.npy')  # 103
z0 = np.load('z0.npy')  # 7
# ---------------------------------------------------------------------
# Model parameters
# ---------------------------------------------------------------------
Nx = 103  # Number of differential states
Nz = 7    # Number of algebraic states
Nd = 2    # Number of disturbances
Nu = 2+1  # Number of inputs: 2 inputs + 1 disturbance (flue gas flow rate [0.8 1.2])
Ny = 23   # Number of outputs
Nw = Nx 
Nv = Ny

tt = [] 
Y = [] 
X = [] 
Z = []
U = [] 

Nsim = 10 # Nt
sigma_w = 0.002
sigma_v = 0.01
random.seed(927)
w = sigma_w*np.random.randn(Nt,Nw)
random.seed(123) 
v = sigma_v*np.random.randn(Nt,Nv)

# initial state
x0_scale = np.zeros(Nx)
delta_x = np.loadtxt('delta.txt')
min_x = np.loadtxt('min.txt')
for i in range(np.size(delta_x)):
    if delta_x[i] <= 1e-10:
        delta_x[i] = 1
        min_x[i] = 0
for j in range(Nx):
    x0_scale[j] = (x0[j]-min_x[j])/delta_x[j]

X += [x0_scale]

#us[0] = us[0] 
#us[1] = us[1]
#us[2] = 1.0 

usim = np.zeros((Nt,Nu))
xsim = np.zeros((Nt,Nx))
zsim = np.zeros((Nt,Nz))
ysim = np.zeros((Nt,Ny))

#### sensor set(measurements)
def measure(x):
    c_extend = np.zeros([Ny,Nx])
    c_extend[0:5,30:35] = np.eye(5)     # C_G,CO2 (absorption)
    c_extend[5:10,45:50] = np.eye(5)    # T_G
    c_extend[10:15,80:85] = np.eye(5)   # C_G,CO2 (desorption)
    c_extend[15:20,95:100] = np.eye(5)  # T_G
    c_extend[20:22,100:102] = np.eye(2) # T (Heat exchanger)
    c_extend[22,102] = np.eye(1)        # T (Reboiler)
    y = np.dot(c_extend,x)
    return y

# ---------------------------------------------------------------------
# Model setup
# ---------------------------------------------------------------------
# Initialize PCC plant
pccplant = PCCPlant.PCCPlant()
# Declare Symbolic variables for DAE system
xd = casadi.SX.sym('x', Nx)  # Differential states
xz = casadi.SX.sym('z', Nz)  # Algebraic states
u = casadi.SX.sym('u', Nu)  # Control inputs
xddot = casadi.SX.sym('xdot', Nx)  # xdot


# Build integrator for plant simulation
ode = pccplant.getODE(xd, xz, u)
alg = pccplant.getALG(xd, xz)
dae = {'x': xd, 'z': xz, 'p': u, 'ode': ode, 'alg': alg}
opts = {"tf": dt, "abstol": 1e-5}  # interval length
plant_sim = casadi.integrator('I', 'idas', dae, opts)

u_max = np.array([0.5812*1.2, 0.1357*1.1, 1*1.1])  
u_min = np.array([0.5812*0.84, 0.1357*0.69, 1*0.4])
for i in range(Nt):
    
    tt += [i*dt/60]  # Current sample time
#    ust = us + 0.0001*np.random.randn(1,3) # us = us + 0.01*np.random.randn(1,3) is variable variance
## the following is input constraint  
#    for j in range (Nu):
#        if ust[:,j] >= u_max[j]:
#            ust[:,j] = u_max[j]
#        if ust[:,j] <= u_min[j]:
#            ust[:,j] = u_min[j]
    U += [u_min]
    
    sol = plant_sim(x0=X[i], z0=z0, p=U[i]) 
    
    X += [sol["xf"].full()[:, 0]]
    Z += [sol["zf"].full()[:, 0]] 
    
X = np.array(X)
Z = np.array(Z)
U = np.array(U)

for i in range(Nt):
    xsim[i,:] = X[i,:] + w[i,:]
    zsim[i,:] = Z[i,:]
    usim[i,:] = U[i,:]
# Number of sampling time
Delta = dt # Sampling time 15min = dt
x_actual = np.zeros((Nt,Nx))      
for i in range(Nx):
    x_actual[:,i] = xsim[:,i] * delta_x[i] + min_x[i]  


F = mpc.getCasadiFunc(pccplant.getODE,[Nx,Nz,Nu],["x","z","u"],"ccmodel")    # “getODE” is the ODE model of pccplant
#F_rk4 = mpc.getCasadiFunc(pccplant.getODE,[Nx,Nz,Nu],["x","z","u"],"ccmodel", rk4=True, Delta=Delta)  # use rk4
H = mpc.getCasadiFunc(measure,[Nx],["x"],"measurement")


print("Calculate the sensitive matrix with all measurements ......")
Tstart = timeit.default_timer()

for t in range(Nt):
    ysim[t,:] = measure(X[t,:])+v[t,:] # Get zero-noise measurement.

sm_all = SenMatrix(F,H,xsim[0:Nsim,:],ysim[0:Nsim,:],usim[0:Nsim-1,:],zsim[0:Nsim-1,:],Nsim-1) 
de_zeros = [0,1,2,3,4,50,51,52,53,54,75,76,77,78,79]
sm_all = np.delete(sm_all,de_zeros,axis=1) 
np.savetxt("sm_all.txt", sm_all)

y_act = np.zeros((Nt,Ny))
for t in range(Nt):
    y_act[t,:] = measure(x_actual[t,:])+v[t,:] 


price = np.array([20, 20, 20, 20, 20, 1, 1, 1, 1, 1, 20, 20, 20, 20, 20, 1, 1, 1, 1, 1, 1, 1, 1]) 
z = np.ones((1,Ny))
cost = np.dot(z,price)

Tend = timeit.default_timer()
print("The sensitive matrix sm = \n",sm_all)
print('Cost time:', str(Tend-Tstart),'s')
#####################################################################

Nx = Nx - 15

#### The sensitive degree with all the measurments:
Tstart = timeit.default_timer()
Flag = SensAnal(sm_all,Nx,Ny,sigma_w,sigma_v,Nsim-1,rank_xp)   
Rank_Sen = Flag[0]
print('Rank of Sensiti:', Rank_Sen)
Sen_deg = Flag[-1]
print("Sen degree:",Sen_deg)
Tend = timeit.default_timer()
print('Cost time:', str(Tend-Tstart),'s')
########################################################


############ If these measurements have been removed:
M_remain = list(range(Ny))
R_known = []  
#R_known = np.array([0,1,11,13,14]) # the removed sensors
N_known = 0  # the number of removed sensors

z_curr = np.zeros((1,Ny))
z_curr[:] = z[:]
z_curr[:,R_known] = 0
cost = np.dot(z_curr,price)
CPI = Sen_deg/cost

print("These measurements have been removed priori:\n", R_known)
for i in R_known:
    M_remain.remove(i)
Ny = Ny - N_known
Nr_max = Nx - N_known

print("Remain these measurements:\n", M_remain)
print("cost:", cost)  
print("CPI:", CPI)

###### calculate the rank and degree
#tf = np.eye(Nsim*(23))
#T_list = []
#        
#for ni in range(Nsim):        # delete Nsim y
#    for nj in R_known:
#        T_list.append(nj+ni*(23))
#tf = np.delete(tf,T_list,axis=0)       
#SM_I = np.dot(tf,sm_all)
#Flag = SensAnal(SM_I,Nx,Ny,sigma_w,sigma_v,Nsim-1,rank_xp)
#Rank_Sen = Flag[0]
#print('Rank of Sensiti:', Rank_Sen)
#Sen_deg = Flag[-1]
#print("Sen degree:",Sen_deg)

################################# begin remove ########################################################
print("------------------------------------------Begin Remove-------------------------------------")

Rank_Sen_final = np.zeros((1,1))
Sen_deg_final = np.zeros((1,1))
cost_final =  np.zeros((1,1))
CPI_final=  np.zeros((1,1))

Rank_Sen_final[0,0] = Rank_Sen
Sen_deg_final[0,0] = Sen_deg
cost_final[0,0] = cost
CPI_final[0,0] = CPI


remove_infor = np.zeros((Nr_max,Ny))

for k in range(Nr_max):
    print("-----------------------------------------------------------------------------------")
    print(k+N_known+1,"- th remove", "(remain:", Nx-N_known,"measurements)")
    Rank_Sen_remain_max = np.zeros((1,1))
    M_remain_try = []
    M_remain_copy = []    
    R_known_try = []
    Ny = Ny - 1
    Sen_deg_opt = np.zeros((1,1))
    cost_opt = np.zeros((1,1))    
    CPI_max = np.zeros((1,1))
    
    for i in M_remain:
        Tstart = timeit.default_timer()
        R_known_try[:] = R_known
        R_known_try.append(i)
        M_remove_try = i;
        
               
        REs = open("Results.txt", "a+")
        print('Try to remove:', M_remove_try)
        print('Try to remove:', M_remove_try, file = REs)
        M_remain_try[:] = M_remain
        M_remain_try.remove(i)
          
        print('Remain these measurements', M_remain_try)
        print('Remain these measurements', M_remain_try, file = REs)
        
        TT = np.eye(Nsim*(23))
        T_list = []
        
        for ni in range(Nsim):        # delete Nsim y
            for nj in R_known_try:
                T_list.append(nj+ni*(23))
        TT = np.delete(TT,T_list,axis=0)       
        SM_I = np.dot(TT,sm_all)

        Flag = SensAnal(SM_I,Nx,Ny,sigma_w,sigma_v,Nsim-1,rank_xp)
        

        Rank_Sen_remain = Flag[0]
        print('Rank of Sensiti:', Rank_Sen_remain)
        print('Rank of Sensiti:', Rank_Sen_remain, file = REs)
#        Sen_state_sort = Flag[1:-1]
#        print("Sen state sort:",Sen_state_sort)
        Sen_deg = Flag[-1]
        
        z_curr[:] = z[:]
        z_curr[:,R_known_try] = 0
        cost = np.dot(z_curr,price)
        CPI = Sen_deg/cost
        
        remove_infor[k,i] = CPI # recoord the CPI in this matrix, in order to plot figure by Matlab
        
        print("Sen degree:",Sen_deg)
        print("Sen degree:",Sen_deg, file = REs)
        print("cost:", cost)  
        print("CPI:", CPI)
        print("CPI:",CPI, file = REs)
        
#        if Rank_Sen_remain_max < Rank_Sen_remain:
#            Rank_Sen_remain_max[0,0] = Rank_Sen_remain 
        
        if CPI >= CPI_max:
           Sen_deg_opt[:] = Sen_deg
           cost_opt[:] = cost
           CPI_max[:] = CPI
           M_remove = M_remove_try
           M_remain_copy[:] = M_remain_try
           Rank_Sen_remain_max[0,0] = Rank_Sen_remain
           
        Tend = timeit.default_timer()
        print('Cost time:', str(Tend-Tstart),'s')
        print("-----------------------------------------------------------------------------------")
        print("-----------------------------------------------------------------------------------", file = REs)
 
       
    if Rank_Sen_remain_max < Nx:  
        print("stop trying")
        print("stop trying", file = REs)
        break
    else:
        if cost_opt <= 93:
            Rank_Sen_final[0,0] = Rank_Sen_remain_max
            Sen_deg_final[0,0] = Sen_deg_opt
            cost_final[0,0] = cost_opt
            CPI_final[0,0] = CPI_max
            
            print("new removed:",M_remove)
            M_remain = M_remain_copy
            R_known.append(M_remove)
            break # End code when the cost condition is met; no break: end code only rank condition
        else:
            Rank_Sen_final[0,0] = Rank_Sen_remain_max
            Sen_deg_final[0,0] = Sen_deg_opt
            cost_final[0,0] = cost_opt
            CPI_final[0,0] = CPI_max
            
        print("new removed:",M_remove)
        M_remain = M_remain_copy
        R_known.append(M_remove)
        print("removed measurements:\n", R_known)
        print("removed measurements:\n", R_known, file = REs)
        print("remains measurements:\n", M_remain)   
        print("remains measurements:\n", M_remain, file = REs) 

        
print("All removed measurements:\n", R_known)
print("All removed measurements:\n", R_known, file = REs)
print("Finally remains:",M_remain) 
print("Rank:",Rank_Sen_final) 
print("Rank:",Rank_Sen_final, file = REs)
print("Degree:",Sen_deg_final)
print("Degree:",Sen_deg_final, file = REs)
print("cost:",cost_final)
print("cost:",cost_final, file = REs)
print("CPI:",CPI_final)
print("CPI:",CPI_final, file = REs)
REs.close()






# ---------------------------------------------------------------------
# Plots and Figures
# ---------------------------------------------------------------------
#fig = plt.figure(1)
#plt.clf()
#plt.subplot(3, 2, 1)
#plt.plot(tt, y_act[:, 0], '--')
#plt.title("CO2 emission conc, M")
#plt.grid
#plt.subplot(3, 2, 2)
#plt.plot(tt, y_act[:, 1], '--')
#plt.title("CO2 produced conc, M")
#plt.grid
#plt.subplot(3, 2, 3)
#plt.plot(tt, y_act[:, 2], '--')
#plt.title("Temp, K")
#plt.grid
#plt.subplot(3, 2, 4)
#plt.plot(tt, y_act[:, 3], '-', drawstyle="steps-post")
#plt.title("Amine vol flow, L/s")
#plt.grid
#plt.subplot(3, 2, 5)
#plt.plot(tt, y_act[:, 4], '-', drawstyle="steps-post")
#plt.title("Heat input, MJ/s")
#plt.grid
#plt.tight_layout()
#fig.savefig('state_control.eps', format='eps', dpi=1200)
#fig.savefig('test.svg', format='svg', dpi=1200)
#fig.savefig('state_control.pdf', format='pdf', dpi=1200)
#
## TypeError: list indices must be integers or slices, not tuple
##X=np.array(X) # use array to transfer X as tuple
##
#fig2 = plt.figure(2)
#plt.clf()
#plt.subplot(2, 1, 1)
#plt.plot(tplot, x_actual[:,30:35], '-')#, marker='o'
#plt.title("")
#plt.grid
#plt.subplot(2, 1, 2)
#plt.plot(tplot, x_actual[:, 45:50], '-')
#plt.title("")
#plt.grid
#plt.tight_layout()
#fig2.savefig('performance.eps', format='eps', dpi=1000)
#fig2.savefig('test.svg', format='svg', dpi=1200)
#fig2.savefig('performance.pdf', format='pdf', dpi=1200)
#
#fig3 = plt.figure(3)
#plt.clf()
#plt.subplot(3, 1, 1)
#plt.plot(tplot, usim[:, 0], '-')
#plt.title("")
#plt.grid
#plt.subplot(3, 1, 2)
#plt.plot(tplot, usim[:, 1], '-')
#plt.title("")
#plt.grid
#plt.subplot(3, 1, 3)
#plt.plot(tplot, usim[:, 2], '-')
#plt.title("")
#plt.grid
#plt.tight_layout()
#fig3.savefig('performance.eps', format='eps', dpi=1000)
#fig3.savefig('test.svg', format='svg', dpi=1200)
#fig3.savefig('performance.pdf', format='pdf', dpi=1200)

# avg_cost = cost.mean()
# fig3 = plt.figure(3)
# plt.clf()
# plt.subplot(1, 1, 1)
# plt.plot(tt, cost[:], '-', marker='o')
# plt.title("Stage cost, $ (Average value: %s)" % avg_cost)
# plt.grid
# plt.tight_layout()
# # fig3.savefig('stage_cost.eps', format='eps', dpi=1200)
# # fig.savefig('test.svg', format='svg', dpi=1200)
# # fig3.savefig('stage_cost.pdf', format='pdf', dpi=1200)
# plt.show()

# print(cost[40:287].mean())

# convert list to array and save

#np.save("t", np.array(tt))
#np.save("X", np.array(X))
#np.save("Y", np.array(Y))
#np.save("Z", np.array(Z))
#np.save("U", np.array(U))
#file = np.load('X.npy')
#print(X)
#np.savetxt("X.txt", X)



     